#!/usr/bin/env python3
"""
@Author: xueyuankui
@Date:   2022-01-22
@email: xueyuankui.good@163.com
@Function: The API used for communication with Airsim simulator
"""
import sys
import time
import threading
import math
import numpy as np

import airsim
from .px4_common import *
from .fcu_interface import FcuInterface


class FcuAirsim(FcuInterface):
    """
    """

    def __init__(self, sim_server_ip, params_dict={}):
        """
        sim_server_ip: the ip address of airsim simulator host
        params_dict: parameters
        """
        super(FcuInterface, self).__init__()
        # connect to the AirSim simulator
        self.client = airsim.MultirotorClient(ip=sim_server_ip)
        if 'drone_type' in params_dict.keys():
            self.drone_type = params_dict['drone_type']
        else:
            self.drone_type = 'drone'
        if 'drone_id' in params_dict.keys():
            self.drone_id = params_dict['drone_id']
        else:
            self.drone_id = 0
        if 'origin_position' in params_dict.keys():
            self.origin_position = params_dict['origin_position']
        else:
            self.origin_position = (0, 0, 0)

        self.drone_name = self.drone_type + "{}".format(self.drone_id)

        # hz
        self.msg_time_sys = {'attitude': 0,
                             'altitude': 0,
                             'local_position_ned': 0,
                             'global_position_int': 0,
                             }
        self.msg_hz = {'attitude': 10,
                       'altitude': 10,
                       'local_position_ned': 20,
                       'global_position_int': 20,
                       }
        # threads
        # thread: receive from fcu
        self.flag_fcu_receive = True
        self.th_fcu_receive = threading.Thread(
            daemon=True, target=self.th_fcu_receive_fun)
        self.th_fcu_receive.start()

    def _get_vehicle_pos(self):
        state = self.client.simGetGroundTruthKinematics()
        x = state.position.x_val
        y = state.position.y_val
        z = state.position.z_val

        x += self.origin_position[0]
        y += self.origin_position[1]
        z += self.origin_position[2]

        return (x, y, z)

    def th_fcu_receive_fun(self):
        while self.flag_fcu_receive:
            for (key, value) in self.msg_time_sys.items():
                if value == 0:
                    self.msg_time_sys[key] = time.time()
                    continue
                delta_time = time.time() - value
                if delta_time >= 1.0/self.msg_hz[key]:
                    self.msg_time_sys[key] = time.time()
                    ###
                    if key == 'attitude':
                        data = self.client.getImuData(
                            vehicle_name=self.drone_name)
                        # linear_acceleration.x_val
                        self.quaternion_NED = data.orientation
                        euler = airsim.utils.to_eularian_angles(
                            data.orientation)
                        # (roll, pitch, yaw)
                        self.attitude_NED = (euler[1], euler[0], euler[2])
                        # print(f"attitude={self.attitude_NED}")

                    elif key == 'local_position_ned':
                        data = self.local_position = self._get_vehicle_pos()
                        # print(f"airsim_api: location_position={self.local_position}")
                        # print(f"location_position={self.local_position[2]}")

                    elif key == 'global_position_int':
                        data = self.client.getGpsData(
                            vehicle_name=self.drone_name)
                        self.global_position = (
                            data.gnss.geo_point.latitude, data.gnss.geo_point.longitude, data.gnss.geo_point.altitude)
                        # self.global_position = (data.latitude, data.longitude, data.altitude)
                        vel = (data.gnss.velocity.x_val,
                               data.gnss.velocity.y_val, data.gnss.velocity.z_val)

                        self.latitude = data.gnss.geo_point.latitude
                        self.longitude = data.gnss.geo_point.longitude
                        self.altitude = data.gnss.geo_point.altitude
                        self.global_vel_x = data.gnss.velocity.x_val
                        self.global_vel_y = data.gnss.velocity.y_val
                        self.global_vel_z = data.gnss.velocity.z_val
                        # print(f"pos={self.global_position}")
                        # print(f"vel={vel}")
                time.sleep(0.01)

    def arm_disarm(self, arm):
        if arm:
            self.client.enableApiControl(True, self.drone_name)
            self.client.armDisarm(True, vehicle_name=self.drone_name)
        else:
            # self.client.enableApiControl(False, self.drone_name)
            self.client.armDisarm(False, vehicle_name=self.drone_name)

    def set_mode(self, custom_mode):
        if custom_mode == PX4_FLIGHT_MODE.TAKEOFF:
            print(f"airsim_api: takeoff")
            self.client.takeoffAsync(vehicle_name=self.drone_name)
        elif custom_mode == PX4_FLIGHT_MODE.LAND:
            print(f"airsim_api: land")
            self.client.landAsync(vehicle_name=self.drone_name)
            print(f"airsim_api: offboard")
        elif custom_mode == PX4_FLIGHT_MODE.OFFBOARD:
            pass

    def setpoint_body_ned(self, type_mask, pvax, pvay, pvaz, yaw_value):
        if type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE:
            self.client.moveByVelocityBodyFrameAsync(
                pvax, pvay, pvaz, 0.05, vehicle_name=self.drone_name)
            if yaw_value != 0:
                self.client.rotateByYawRateAsync(math.degrees(
                    yaw_value), 0.05, vehicle_name=self.drone_name)

        elif type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAW:
            self.client.moveByVelocityBodyFrameAsync(
                pvax, pvay, pvaz, 0.1, vehicle_name=self.drone_name)
            self.client.rotateToYawAsync(math.degrees(
                yaw_value), 0.1, vehicle_name=self.drone_name)

    def setpoint_local_ned(self, type_mask, pvax, pvay, pvaz, yaw_value):
        if type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAWRATE:
            self.client.moveByVelocityAsync(
                pvax, pvay, pvaz, 0.1, vehicle_name=self.drone_name)
            if yaw_value != 0:
                self.client.rotateByYawRateAsync(
                    yaw_value, 0.1, vehicle_name=self.drone_name)

        elif type_mask == PX4_TYPE_MASK.MASK_NED_VEL_YAW:
            self.client.moveByVelocityAsync(
                pvax, pvay, pvaz, 0.1, vehicle_name=self.drone_name)
            self.client.rotateToYawAsync(
                yaw_value, 0.1, vehicle_name=self.drone_name)

    def setpoint_global_ned_alt(self, type_mask, pvax, pvay, pvaz, yaw_value):
        pass

    def cmd_goto_global_position(self, lat, lon, alt, yaw, speed):
        self.client.moveToGPSAsync(lat, lon, alt, speed, 0.1)

    def exit(self):
        try:
            self.client.enableApiControl(False, vehicle_name=self.drone_name)
        except:
            pass
